#include "HeadInstructionHandler.h"


HeadInstructionHandler::HeadInstructionHandler(AiboState* aiboState)
{
	this->setAiboState( aiboState );
	m_HeadInstructionList = new QVector<HeadInstruction>();
}

HeadInstructionHandler::~HeadInstructionHandler(void)
{
}

void HeadInstructionHandler::updateAiboState() {
	if( !this->getInstructionList()->isEmpty() ) {
		qDebug() << "HeadInstructions: Updating AiboState - Instructions left: " << this->getInstructionList()->size();
		HeadInstruction instruction = this->getInstructionList()->back();
		this->getInstructionList()->pop_back();
		this->sendGeneralInstruction( &instruction );
	}
}

bool HeadInstructionHandler::addInstruction( HeadInstruction wInstruction ) {
	bool success = true;
	if( !this->getInstructionList()->isEmpty() ) {
		if( this->getInstructionList()->front().getType() != wInstruction.getType() ) {
			this->getInstructionList()->push_front( wInstruction );
		}
		else if (this->getInstructionList()->front().getValue() != wInstruction.getValue() ) {
				this->getInstructionList()->push_front( wInstruction );
		}
		else
			success = false;
	}
	else {
		this->getInstructionList()->push_front( wInstruction );
	}
	return success;
}

void HeadInstructionHandler::resetAiboState() 
{
	qDebug() << "HeadInstructions: Updating AiboState";
	addInstruction( HeadPanInstruction( 0.0f ) );
	addInstruction( HeadRollInstruction( 0.0f ) );
	addInstruction( HeadTiltInstruction( 0.0f ) );
}

void HeadInstructionHandler::sendGeneralInstruction( HeadInstruction* instruction )
{
	switch( instruction->getType() ) {
	case CMD_HEADPOINT_PAN:
		sendInstruction( (HeadPanInstruction*)instruction );
		break;
	case CMD_HEADPOINT_ROLL:
		sendInstruction( (HeadRollInstruction*)instruction );
		break;
	case CMD_HEADPOINT_TILT:
		sendInstruction( (HeadTiltInstruction*)instruction );
		break;
	}
}

bool HeadInstructionHandler::sendInstruction( HeadPanInstruction* panInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL && panInstruction != NULL ) {
		if( !this->getAiboState()->isHeadInstructionOn() ) {
			qDebug() << "HeadInstructions: Pan On - No Previous Instructions";
			success = true;
		}
		else if( this->getAiboState()->isPanning() && this->getAiboState()->getPan() != panInstruction->getValue() ) {
			qDebug() << "WalkInstructions: Pan On - Previous Speed Changed";
			success = true;
		}
		else {
			this->addInstruction( *panInstruction );
		}
		if( success )
			this->getAiboState()->setPan( panInstruction->getValue() );
		else
			qDebug() << "HeadInstructions: No Pan Msg Sent";
	}
	return success;
}

bool HeadInstructionHandler::sendInstruction( HeadRollInstruction* rollInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL && rollInstruction != NULL ) {
		if( !this->getAiboState()->isHeadInstructionOn() ) {
			qDebug() << "HeadInstructions: Roll On - No Previous Instructions";
			success = true;
		}
		else if( this->getAiboState()->isRolling() && this->getAiboState()->getRoll() != rollInstruction->getValue() ) {
			qDebug() << "WalkInstructions: Roll On - Previous Speed Changed";
			success = true;
		}
		else {
			this->addInstruction( *rollInstruction );
		}
		if( success )
			this->getAiboState()->setRoll( rollInstruction->getValue() );
		else
			qDebug() << "WalkInstructions: No Roll Msg Sent";
	}
	return success;
}

bool HeadInstructionHandler::sendInstruction( HeadTiltInstruction* tiltInstruction ) {
	bool success = false;
	if( this->getAiboState() != NULL && tiltInstruction != NULL ) {
		if( !this->getAiboState()->isHeadInstructionOn() ) {
			qDebug() << "HeadInstructions: Tilt On - No Previous Instructions";
			success = true;
		}
		else if( this->getAiboState()->isTilting() && this->getAiboState()->getTilt() != tiltInstruction->getValue() ) {
			qDebug() << "HeadInstructions: Tilt On - Previous Speed Changed";
			success = true;
		}
		else {
			this->addInstruction( *tiltInstruction );
		}
		if( success )
			this->getAiboState()->setTilt( tiltInstruction->getValue() );
		else
			qDebug() << "HeadInstructions: No Tilt Msg Sent";
	}
	return success;
}
